#include "algo_math.h"

void QuickSort(int32 *i32p_arr, int32 i32_startPos, int32 i32_endPos)
{
    int32 i, j;
    int32 key;
    key = i32p_arr[i32_startPos];
    i = i32_startPos;
    j = i32_endPos;
    if (i<j)
    {
        while (i<j)
        {
            for ( ; i32p_arr[j] >= key && i<j; --j )
                ;
            i32p_arr[i] = i32p_arr[j];
            for ( ; i32p_arr[i] <= key && i<j; ++i)
                ;
            i32p_arr[j] = i32p_arr[i];
        }
        i32p_arr[i] = key;
        QuickSort(i32p_arr, i32_startPos, i - 1);
        QuickSort(i32p_arr, i + 1, i32_endPos);
    }
}


fixed32 WeightMean(fixed32 f32_value1, fixed32 f32_value2, fixed32 f32_weight1)
{
    if ( f32_weight1<0 || f32_weight1>PERCISION )
        f32_weight1 = PERCISION/2;
    f32_value1 = f32_value1*f32_weight1 + f32_value2*(PERCISION-f32_weight1);
    f32_value1 = S_DIV( f32_value1, PERCISION );
    return f32_value1;
}

fixed32 Exp(fixed32 f32_exponent )
{
    uint32 i;
    uint32 u32_extraScale = 10,
           u32_totalScale = u32_extraScale * PERCISION,
           u32_delta = 1 * u32_extraScale,
           u32_ret = 1 * u32_totalScale;

    if ( f32_exponent < 0 )
        return U_DIV( PERCISION*PERCISION, Exp(-f32_exponent) );

    for ( i=1; i<32; i++ )
    {
        u32_delta = U_DIV( u32_delta*f32_exponent, i );
        if ( u32_delta==0 || u32_ret/u32_delta>u32_totalScale ) break;
        u32_ret += u32_delta;
        u32_delta = U_DIV( u32_delta, PERCISION);
    }
    u32_ret = U_DIV( u32_ret, u32_extraScale );
    return u32_ret;
}

fixed32 Log( fixed32 f32_antilog )
{
    uint32 i;
    fixed32 f32_ln10 = 23026*PERCISION/10000;
    uint32 u32_extraScale = 10,
           u32_totalScale = u32_extraScale * PERCISION,
           u32_numerator,
           u32_delta,
           u32_ret = 0;

    if ( f32_antilog <= 0)
        return 0;
    if ( f32_antilog < PERCISION )
        return Log( f32_antilog*10 ) - f32_ln10;
    else if ( f32_antilog / PERCISION >= 10)
        return Log( f32_antilog/10 ) + f32_ln10;

    f32_antilog = U_DIV( (f32_antilog-PERCISION)*u32_totalScale, (f32_antilog+PERCISION) );
    u32_numerator = f32_antilog;
    f32_antilog = U_DIV ( f32_antilog*f32_antilog, u32_totalScale );
    for ( i=0; i<32; i++ )
    {
        u32_delta = U_DIV( u32_numerator, (2*i+1) );
        if ( u32_delta==0 || u32_ret/u32_delta>u32_totalScale )
            break;
        u32_ret += u32_delta;
        u32_numerator = U_DIV( u32_numerator*f32_antilog, u32_totalScale );
    }
    u32_ret = U_DIV( u32_ret*2, u32_extraScale );
    return u32_ret;
}

fixed32 Pow( fixed32 f32_base, fixed32 f32_exponent )
{
    if (f32_base == 0)
        return 0;
    f32_exponent *= Log(f32_base);
    f32_exponent = S_DIV( f32_exponent, PERCISION );
    return Exp( f32_exponent);
}

uint32 Sqrt( uint32 u32_Value )
{
    uint32 u32_Root = 0,
           u32_Remainder = u32_Value,
           u32_Place = 0x40000000;

    while (u32_Place > u32_Remainder)
        u32_Place = u32_Place >> 2;
    while (u32_Place)
    {
        if (u32_Remainder >= u32_Root+u32_Place)
        {
            u32_Remainder -= u32_Root+u32_Place;
            u32_Root += u32_Place << 1;
        }
        u32_Root = u32_Root >> 1;
        u32_Place = u32_Place >> 2;
    }

    if (u32_Value-u32_Root*u32_Root > u32_Root)
        u32_Root += 1;
    return u32_Root;
}

fixed32 Real(comp64 c64_value)
{
    return (fixed32)(c64_value >> 32);
}

fixed32 Imag(comp64 c64_value)
{
    return (fixed32)(c64_value & 0xFFFFFFFF);
}

fixed32 Modulus(comp64 c64_value)
{
    fixed32 f32_real = Real(c64_value);
    fixed32 f32_imag = Imag(c64_value);
    return Sqrt(f32_real*f32_real + f32_imag*f32_imag);
}
